#ifndef DEPTH_CAMERA_HPP
#define DEPTH_CAMERA_HPP

#include <rclcpp/rclcpp.hpp>
#include <tinyxml2.h>
#include <Eigen/Dense>
#include <limits>
#include <iostream>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
// #include <std_msgs/msg/time.hpp>
#include <builtin_interfaces/msg/time.hpp>
#include <agv_msgs/msg/avoid_obsta.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/pca.h>
#include <pcl/common/common.h>
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <scenes_judge.hpp>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <vector>
#include <string>

namespace AgvObstaAvoid {

class DepthCamera
{
public:
    struct zPoint {
        float x;
        float y;
        float zMin;
        float zMax;
    };

    struct Point {
        float x;
        float y;
        float z;
    };

    struct ExtrinsicParameter {
        double x;
        double y;
        double z;
        double roll;
        double pitch;
        double yaw;
        float  crop_xmin;
        float  crop_ymin;
        float  crop_zmin;
        float  crop_xmax;
        float  crop_ymax;
        float  crop_zmax;
    };

    struct uRegion {
        std::string name;
        std::vector<zPoint> poinlist;
    };

    struct uArea {
        int name;
        std::vector<uRegion> regionlist;
    };

    struct uEquipment {
        int name;
        std::string topicname;
        ExtrinsicParameter ex_params;
        std::unordered_map<int, uArea> arealist;
    };

    struct PolygonCache {
        float min_x, max_x, min_y, max_y, min_z, max_z;
        std::vector<zPoint> points;
    };

    struct OBB {
        Eigen::Vector3f center;
        Eigen::Vector3f axes[3];
        Eigen::Vector3f extents;
    };

    struct AABB {
        float min_x, max_x;
        float min_y, max_y;
        float min_z, max_z;
    };

    explicit DepthCamera(rclcpp::Node::SharedPtr node);
    ~DepthCamera() = default;

    bool CameraReadXml(tinyxml2::XMLElement *element);
    std::vector<agv_msgs::msg::AvoidObsta> CloudCheck(std::vector<ScenesJudge::cEquip> &camera_equip);
    
    void init();
    void ShowEquipParam(uEquipment *equipment);
    void CloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg, 
                      const ExtrinsicParameter &ex, const int& equip_name, int topic_name);
    bool Isenable(std::string status, ScenesJudge::cEquip &equip);
    bool InPolygon(const Point &p, const std::vector<zPoint> &poly);
    std::vector<zPoint> RegionSeg(const int& equip_name, const int& area_name, const std::string& region_name);
    void PrecomputeBoundingBox(std::vector<zPoint>& poly, PolygonCache& cache);
    bool FastInPolygon(const Point& p, const PolygonCache& cache);
    bool TimeoutCheck(const builtin_interfaces::msg::Time& time);
    bool ObbIntersect(const OBB& a, const OBB& b);
    void ComputeOBB(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, OBB& obb);
    AABB ComputeAABB(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
    bool AabbCollision(const AABB& box1, const AABB& box2);
    
    std::unordered_map<int, std::vector<Point>> scan_point;
    std::vector<uEquipment> camera_equip_list;
    rclcpp::Node::SharedPtr node_;
private:
    std::mutex cloud_mutex;
    
    // ROS2 Subscribers and Publishers
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_1;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_2;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_3;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr obstacle_pub_1;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr obstacle_pub_2;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr obstacle_pub_3;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub;
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr depth_camera_info_sub;

    builtin_interfaces::msg::Time lidar_time_1;
    builtin_interfaces::msg::Time lidar_time_2;
    builtin_interfaces::msg::Time lidar_time_3;
};

} // namespace AgvObstaAvoid

#endif // DEPTH_CAMERA_HPP